#ifndef GNSS_H
#define GNSS_H

#include "utm_convert/utm.h"
#include <sensor_msgs/NavSatFix.h>
constexpr double kDEG2RAD = M_PI / 180.0;  // deg->rad
constexpr double kRAD2DEG = 180.0 / M_PI;  // rad -> deg

struct GNSS {
    // 默认构造函数
    GNSS() : timestamp(0.0), latitude(0.0), longitude(0.0), altitude(0.0),
             utm_x(0.0), utm_y(0.0), utm_z(0.0), valid(false) {}

    // 从NavSatFix消息初始化
    void fromNavSatFix(const sensor_msgs::NavSatFix::ConstPtr& msg) {
        timestamp = msg->header.stamp.toSec();
        latitude = msg->latitude;
        longitude = msg->longitude;
        altitude = msg->altitude;
//        valid = (msg->status.status >= sensor_msgs::NavSatStatus::STATUS_FIX);
        valid = 1;

        // 转换为UTM51N坐标系
        long zone = 0;
        char char_north = 0;
        double utm_E;
        double utm_N;

        long ret = Convert_Geodetic_To_UTM(latitude * kDEG2RAD, longitude * kDEG2RAD, &zone, &char_north,
                                           &utm_E, &utm_N);

        utm_x = utm_E;
        utm_y = utm_N;
        utm_z = altitude;

        // 设置第一个有效RTK坐标作为原点
        if(valid && !origin_set) {
            origin << utm_x, utm_y, altitude;
            origin_set = true;
        }
    }

    // 获取相对于原点的坐标
    Eigen::Vector3d getRelativePosition() const {
        return Eigen::Vector3d(utm_x - origin[0],
                               utm_y - origin[1],
                               utm_z - origin[2]);
    }

    // 成员变量保持公共访问权限
    double timestamp;     // 时间戳
    double latitude;      // 纬度
    double longitude;     // 经度
    double altitude;      // 海拔
    double utm_x;        // UTM X坐标
    double utm_y;        // UTM Y坐标
    double utm_z;        // UTM Z坐标
    bool valid;          // 数据有效标志

    static Eigen::Vector3d origin;
    static bool origin_set;
};

// 静态成员变量初始化
Eigen::Vector3d GNSS::origin = Eigen::Vector3d::Zero();
bool GNSS::origin_set = false;

using GNSSPtr = std::shared_ptr<GNSS>;

#endif
